#include "as5600.h"               

float angle_prev=0;


/**
  * @brief  AS5600读取寄存器
  * @param  reg 寄存器地址
  * @param  buf 寄存器数据写入的缓冲区
  * @return 无
  * @author Cc
  */
void as5600_read_reg(uint16_t reg, uint8_t* buf)
{
	HAL_I2C_Mem_Read(&hi2c1, AS5600_ADDRESS, reg, I2C_MEMADD_SIZE_8BIT, buf, 1, 100);
}
 
/**
  * @brief  读取弧度制的角度 0 ~ 6.28
  * @param  无
  * @return angle 弧度制的角度
  * @author Cc
  */
float getangle_without_track(void)
{
    float angle = 0.0;
    uint8_t datah = 0;
    uint8_t datal = 0;
    
    /* 读取角度高字节和低字节 */
    as5600_read_reg(AS5600_RAW_ANGLE_H,&datah);
    as5600_read_reg(AS5600_RAW_ANGLE_L,&datal);
    
    /* 组合成16位角度值 */
    angle = (datah << 8) | datal;
    
    /* 转换为弧度值（0-2π） */
    angle = (angle / 4096) * _2PI;
    return angle;
}
 
 
/**
  * @brief  读取角度
  * @param  无
  * @return angle 弧度制的角度
  * @author Cc
  */
float get_angle(void)
{
    static float last_angle = 0.0;
    static float full_rotations = 0.0;              /* 完整旋转圈数 */
    float angle = getangle_without_track();         /* 获取当前角度 */
    float angle_d = angle - last_angle;             /* 计算角度变化量 */   
    
    /* 检测是否转完一圈 */
    if (fabs(angle_d) > (0.8f * _2PI))full_rotations = full_rotations + ((angle_d > 0) ? -1 : 1);   
   
    last_angle = angle;                             /* 更新上次角度值 */
    
    return (full_rotations * _2PI + last_angle);  /* 返回带圈数的角度 */
}
